package com.googlecode.curvedmetromaps.core.geometry;

import org.jbox2d.common.Vec2;

import com.googlecode.curvedmetromaps.core.model.MetroMap;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

public class BasicGeometry {

	/**
	 * Delta angle function. Calculates the angle between a line through 0-1 and
	 * 1-2. If you stand at p0 and look at p1, the result is negative when p2 is
	 * to the right and positive when p2 is to the left. When p0, p1, p2 are
	 * colinear the result is 0.
	 * @return the angle between a line through 0-1 and 1-2, in the range (-pi,
	 *         pi].
	 */
	public static double dAngle(Vec2 p0, Vec2 p1, Vec2 p2) {
		Vec2 p10 = p0.sub(p1);
		Vec2 p12 = p2.sub(p1);

		// Angle between p10 and x-axis
		double theta10 = Math.atan2(p10.y, p10.x);
		// Angle between p12 and x-axis
		double theta12 = Math.atan2(p12.y, p12.x);

		double theta = theta12 - theta10;

		// Calculate difference from straight line.
		if (theta > 0f)
			return theta - Math.PI;
		else
			return theta + Math.PI;
	}

	/**
	 * Point triangle projection.
	 * @return point on the triangle defined by u,v,w closest to p.
	 */
	public static Vec2 pointTriangleProjection(Vec2 p, Vec2 u, Vec2 v, Vec2 w) {
		Vec2 uv = pointLineSegmentProjection(p, u, v);
		Vec2 vw = pointLineSegmentProjection(p, v, w);
		Vec2 wu = pointLineSegmentProjection(p, w, u);

		float distUv = uv.lengthSquared();
		float distVw = vw.lengthSquared();
		float distWu = wu.lengthSquared();

		if (distUv < distVw) {
			if (distUv < distWu)
				return uv;
			else
				return wu;
		} else {
			if (distVw < distWu)
				return vw;
			else
				return wu;
		}
	}

	/**
	 * Point triangle distance.
	 * @return point on the triangle defined by u,v,w closest to p.
	 */
	public static float pointTriangleDistance(Vec2 p, Vec2 u, Vec2 v, Vec2 w) {
		return Math.abs(pointTriangleProjection(p, u, v, w).length());
	}

	/**
	 * Point line projection.
	 * @return Vector from p0 to the point on the line through p1 and p2 closest to p0.
	 */
	public static Vec2 pointLineProjection(Vec2 p0, Vec2 p1, Vec2 p2) {
		Vec2 wv = p2.sub(p1);
		final float l2 = wv.lengthSquared();
		if (l2 == 0f)
			return p1.sub(p0); // p0 == p1 case
		final float t = Vec2.dot(p0.sub(p1), wv) / l2;
		final Vec2 projection = p1.add((wv).mul(t)); // Projection falls on the
														// segment
		return projection.sub(p0);
	}

	/**
	 * Point line distance.
	 * @return distance from the point on the line through p1 and p2 closest to
	 *         p0.
	 */
	public static double pointLineDistance(Vec2 p0, Vec2 p1, Vec2 p2) {
		return Math.abs(pointLineProjection(p0, p1, p2).length());
	}

	/**
	 * Point line segment projection.
	 * @return point on the line segment from p1 to p2 closest to p0.
	 */
	public static Vec2 pointLineSegmentProjection(Vec2 p0, Vec2 p1, Vec2 p2) {
		Vec2 wv = p2.sub(p1);
		final float l2 = wv.lengthSquared();
		if (l2 == 0f)
			return p1.sub(p0); // p0 == p1 case
		// Consider the line extending the segment, parameterized as v + t (w -
		// v).
		// We find projection of point p onto the line.
		// It falls where t = [(p-v) . (w-v)] / |w-v|^2
		final float t = Vec2.dot(p0.sub(p1), wv) / l2;
		if (t < 0.0)
			return p1.sub(p0); // Beyond the 'v' end of the segment
		else if (t > 1.0)
			return p2.sub(p0); // Beyond the 'w' end of the segment
		final Vec2 projection = p1.add((wv).mul(t)); // Projection falls on the
														// segment
		return projection.sub(p0);
	}

	/**
	 * Point line segment distance.
	 * @return distance from p0 to the closest point on the line through p1 and
	 *         p2.
	 */
	public static double pointLineSegmentDistance(Vec2 p0, Vec2 p1, Vec2 p2) {
		return pointLineSegmentProjection(p0, p1, p2).length();
	}

	public static float[] getBoundingBox(MetroStation[] stations) {
		if (stations.length == 0)
			return null;

		// Calculate bounding box of model
		MetroStation first = stations[0];
		float xMin = first.x;
		float xMax = first.x;
		float yMin = first.y;
		float yMax = first.y;
		for (int i = 1; i < stations.length; i++) {
			MetroStation m = stations[i];

			if (m.x < xMin)
				xMin = m.x;
			else if (m.x > xMax)
				xMax = m.x;

			if (m.y < yMin)
				yMin = m.y;
			else if (m.y > yMax)
				yMax = m.y;
		}
		
		float[] boundingBox = { xMin, yMin, xMax, yMax };
		return boundingBox;
	}

	/**
	 * Atan2 approximation with an error of at most +/- 0.00001.
	 */
	public static float atan2(float y, float x) {
		float t0, t1, t2, t3;

		t2 = Math.abs(x);
		t1 = Math.abs(y);
		t0 = Math.max(t2, t1);
		t1 = Math.min(t2, t1);
		t2 = 1f / t0;
		t2 = t1 * t2;

		t3 = t2 * t2;
		t0 = -0.013480470f;
		t0 = t0 * t3 + 0.057477314f;
		t0 = t0 * t3 - 0.121239071f;
		t0 = t0 * t3 + 0.195635925f;
		t0 = t0 * t3 - 0.332994597f;
		t0 = t0 * t3 + 0.999995630f;
		t2 = t0 * t2;

		t2 = (Math.abs(y) > Math.abs(x)) ? 1.570796327f - t2 : t2;
		t2 = (x < 0) ? 3.141592654f - t2 : t2;
		t2 = (y < 0) ? -t2 : t2;

		return t2;
	}

	/**
	 * Translate and scale the metro map such that it lies in the origin and has
	 * width 1.
	 */
	public static void normalizeMap(MetroMap metroMap) {
		if (metroMap.stations.length == 0)
			return;

		// Calculate bounding box of model
		MetroStation first = metroMap.stations[0];
		float xMin = first.x;
		float xMax = first.x;
		float yMin = first.y;
		float yMax = first.y;
		for (int i = 1; i < metroMap.stations.length; i++) {
			MetroStation m = metroMap.stations[i];

			if (m.x < xMin)
				xMin = m.x;
			else if (m.x > xMax)
				xMax = m.x;

			if (m.y < yMin)
				yMin = m.y;
			else if (m.y > yMax)
				yMax = m.y;
		}

		float w = (xMax - xMin);
		float h = (yMax - yMin);
		float a = w * h;
		
		float scale = 1;
		if (a != 0)
			scale = (float) Math.sqrt(1 / a);
		else if (w != 0)
			scale = 1 / w;
		else if (h != 0)
			scale = 1 / h;

		// Translate and scale map
		metroMap.translate(-xMin, -yMin);
		if (xMax > xMin)
			metroMap.scale(scale);
	}

}
